#include "user_task.h"
#include "navi.h"
#ifdef DEBUG
ops_t o;
Action_POS_t pos_get;
CMD_ACTION_t ops_out;//经过导航算法解算后输出的期望控制值
#else

static ops_t o;
static Action_POS_t pos_get;
static CMD_ACTION_t ops_out;

#endif


void Task_Navi(void *argument){
	
	 (void)argument;
   
   const uint32_t delay_tick = osKernelGetTickFreq() /TASK_FREQ_NAVI;

    Action_init(&o,&(task_runtime.config->ops),&pos_get);

   uint32_t tick = osKernelGetTickCount(); /* 获取当前控制任务运行频率的tick*/

    while (1)
   {
#ifdef DEBUG
    /* 记录任务使用的的栈空间 */
    task_runtime.stack_water_mark.action =
        osThreadGetStackSpace(osThreadGetId());
#endif
    //获取需要在此任务中使用的其他任务中的状态量
     osMessageQueueGet(task_runtime.msgq.cmd.raw.ops_9pos,&pos_get,NULL,0);
		 
     osMessageQueueGet(task_runtime.msgq.imu.gyro,&o.ops_gyro,NULL,0);
		 
//	osMessageQueueGet(task_runtime.msgq.imu.eulr,&o.ops_imu_pos,NULL,0);

     go_path(&o,&ops_out);

    //将解算后的导航期望运动值放入消息队列供其他任务使用
    osMessageQueueReset(task_runtime.msgq.cmd.raw.ops_9);
    osMessageQueuePut(task_runtime.msgq.cmd.raw.ops_9,(&ops_out),0,0);


	    tick += delay_tick; /* 计算下一个唤醒时刻*/
	    osDelayUntil(tick);
   }

}
	
	
	
	
	
	
	
	
	
	
	



